/*
 * forage.java
 */

import java.util.Random;
import java.util.concurrent.locks.ReentrantLock;
import	EDU.gatech.cc.is.util.Vec2;
import	EDU.gatech.cc.is.abstractrobot.*;

public class Controller  extends ControlSystemMFN150
{
	
//=== STATIC MEMBERS ==============================================	
	protected final String PHD_FOLDER = "../../.."; 
	protected static ReentrantLock synchron = new ReentrantLock();
	public final static boolean DEBUG = true;
	
//=== LOCAL MEMBERS ===============================================
	protected MultiForageN150 bot;
	protected long timestamp = 0;
	
//=== OUTPOUT FUNCTIONS ===========================================
	static String F(double f){ return String.format("%.2f", f); }
	void print(String s){
		synchron.lock();
		abstract_robot.setDisplayString(s);
		System.out.println("BOT "+abstract_robot.getID()+" STATE: "+s);
		synchron.unlock();
	}

//=== MATH UTILITIES ==============================================
//=== --- Vectors -------------------------------------------------
	Vec2 copy(Vec2 v){ return new Vec2(v.x, v.y); }
	Vec2 sub(Vec2 a, Vec2 b){ Vec2 x = copy(a); x.sub(b); return x; }
	Vec2 add(Vec2 a, Vec2 b){ Vec2 x = copy(a); x.add(b); return x; }

//=== --- probabilities and randomization -------------------------	
	boolean inCases(int n, int N){
		return Math.random()<(n/N);
	}
	boolean inCases(float n){
		double rnd = Math.random();
		//System.out.println("rnd = "+rnd+", n="+n);
		return rnd<n;
	}
	Random _random = new Random();
	double rnd(){
		return _random.nextDouble(); 
	}
	double rnd(double min, double max){ 
		return rnd()*(max-min) + min; 
	}

//=== --- Time ----------------------------------------------------	
	class Timer{
		long time;
		long lastTime;
		public Timer(long time){ this.time=time; lastTime=timestamp; }
		boolean isTime(){ 
			boolean res = (timestamp - lastTime)>=time;
			if(res){ lastTime = timestamp; return true; }
			return false;
		}
	}
		
	
//=== ROBOT INTERFACE FUNCTIONS ===================================
	void bot_turn(long t, double ang){
		bot.setSteerHeading(t, bot.getSteerHeading(t)+ang);
		bot.setTurretHeading(t, bot.getTurretHeading(t)+ang);
	}
	void bot_turn(double ang){
		bot_turn(timestamp, ang);
	}
	Vec2D myPos(){ 
		return new Vec2D(bot.getPosition(timestamp)); 
	}
	double heading(){
		return Vec2D.polar(bot.getSteerHeading(timestamp),1).h();
	}
	double turret(){
		return Vec2D.polar(bot.getTurretHeading(timestamp),1).h();
	}
	Vec2D.Pose myPose(){ return new Vec2D.Pose(myPos(), heading()); }

//=== SENSORS ANALISYS FUNCTIONS ==================================	
	
	Vec2 getClosest_v1(Vec2[] vec, Vec2 p){
		if(vec.length==0) return null;
		Vec2 c = vec[0];
		for(Vec2 v: vec){
			Vec2 vv = new Vec2(v); //vv.sub(p);
			Vec2 cc = new Vec2(c); //cc.sub(p);
			if(vv.r<cc.r) c = v;
		}
		Vec2 res = new Vec2(c); res.add(p);
		return res;
	}
	Vec2 getClosest_v2(Vec2[] vec, Vec2 p){
		if(vec.length==0) return null;
		Vec2 c = vec[0];
		Vec2 res = new Vec2(c); res.add(p);
		return res;
	}
	Vec2 getClosest(Vec2[] vec, Vec2 p){
		return  getClosest_v1(vec, p); 
	}
	
	Vec2D closestObstacle(){ 
		Vec2 v = getClosest(bot.getObstacles(timestamp), bot.getPosition(timestamp));
		if(v==null) return null;
		return new Vec2D(v);
	}
	Vec2D closestRobot(){ 
		Vec2 v = getClosest(bot.getTeammates(timestamp), bot.getPosition(timestamp));
		//Vec2 v = getClosest(bot.getVisualObjects(timestamp, 0xC0C0C0), bot.getPosition(timestamp));
		if(v==null) return null;
		return new Vec2D(v);
	}
	

	public void configure()
	{		
		super.configure();
		
		bot = abstract_robot;
		bot.setObstacleMaxRange(5.0); 
		bot.setBaseSpeed(0.4*SimpleN150.MAX_TRANSLATION);
		bot.setGripperHeight(-1,0);   
		bot.setGripperFingers(-1,1);  
	
	}

//=== ITERATION CONTROLS =========================================
	protected int stepCounter = 0;	
	protected static boolean TrialInProgress = false;
	
//=== --- TRIALS -------------------------------------------------
	protected void onTeamTrialInit(){
		
	}
	protected void onTeamTrialEnd(){
		
	}
	protected void onTrialInit(){
		
	}
	protected void onTrialEnd(){
		
	}
	public void trialInit(){
		super.trialInit();
		
		if(TrialInProgress==false){
			System.out.println("START");
			onTeamTrialInit();
		}
		TrialInProgress = true;
		stepCounter=0;

		onTrialInit();
	}

	public void trialEnd(){
		super.trialEnd();
		
		if(TrialInProgress){
			System.out.println("END");
			onTeamTrialEnd();
		}
		TrialInProgress = false;
		System.out.println("steps = "+stepCounter);

		onTrialEnd();
	}

//=== --- STEPS --------------------------------------------------	

	public int takeStep()
	{
		super.takeStep();
		stepCounter++;
		if(bot.getID()==1){
			System.out.println("Step #"+stepCounter);
		}
		long	t = abstract_robot.getTime();
		timestamp = t;
		
		Vec2D mypos = myPos();
		double h = heading();
		print("pos: "+mypos.x()+","+mypos.y()+","+h);

		
		return(CSSTAT_OK);
		}
	
}
